#include "mathengine.h"

#include <gtest/gtest.h>

namespace {

using namespace mathengine;

class VecTest : public testing::Test {
protected:
    void SetUp() override {
        v1 = {1};
        v2 = {1, 2};
        v3 = {1, 2, 3};
        v4 = {4, 5, 6};
    }

    void TearDown() override {
    }

    Vec3f v1;
    Vec3f v2;
    Vec3f v3;
    Vec3f v4;
};

TEST_F(VecTest, DefaultConstructor) {
    EXPECT_EQ(Vec4f::zero(), Vec4f::zero());
    EXPECT_EQ(Vec4f({0, 0, 0, 0}), Vec4f::zero());
    EXPECT_EQ(Vec3f({1, 2, 3}), Vec3f({1, 2, 3}));
    EXPECT_NE(Vec3f({1, 2, 3}), Vec3f::zero());
    EXPECT_NE(Vec3f({1}), Vec3f({1, 1, 1}));
    EXPECT_EQ(Vec3f(1), Vec3f({1, 1, 1}));
    EXPECT_EQ(Vec3f({1, 1}), Vec3f({1, 1, 0}));
    float arr[3] = {1, 2, 3};
    Vec3f t0({1, 2, 3});
    Vec3f t1(arr);
    Vec3f t2{3, 2, 1};
    Vec3f t3{-1, -2, -3};
    EXPECT_EQ(t0, t1);
    EXPECT_NE(t1, t2);
    EXPECT_NE(t1, t3);
}

TEST_F(VecTest, Accessor) {
    EXPECT_EQ(v1.x(), 1);
    EXPECT_EQ(v1.y(), 0);
    EXPECT_EQ(v1.z(), 0);

    EXPECT_EQ(v2.x(), 1);
    EXPECT_EQ(v2.y(), 2);
    EXPECT_EQ(v2.z(), 0);

    EXPECT_EQ(v3.x(), 1);
    EXPECT_EQ(v3.y(), 2);
    EXPECT_EQ(v3.z(), 3);
}

TEST_F(VecTest, Normalize) {
    EXPECT_EQ(v1.length(), 1);
    EXPECT_EQ(v1.norm(), 1);
    EXPECT_EQ(v1.norm(), 1);

    auto t1 = v1;
    EXPECT_EQ(t1.normalize(), Vec3f({1, 0, 0}));
}

TEST_F(VecTest, AddAndSub) {
    EXPECT_NE(v1, Vec3f::zero());
    EXPECT_EQ(v1 - v1, Vec3f::zero());

    EXPECT_NE(v3, Vec3f::zero());
    EXPECT_EQ(v3 - v3, Vec3f::zero());
    EXPECT_EQ(v3 + Vec3f::zero(), v3);
    EXPECT_EQ(v3 + v4, Vec3f({5, 7, 9}));
    EXPECT_EQ(v4 + v3, Vec3f({5, 7, 9}));
    EXPECT_EQ(v3 - v4, Vec3f({-3, -3, -3}));
    EXPECT_EQ(v4 - v3, Vec3f({3, 3, 3}));
}

TEST_F(VecTest, NumMultiplyAndNumDiv) {
    EXPECT_EQ((0 * v3), Vec3f::zero());
    EXPECT_EQ((v3 * 0), Vec3f::zero());
    EXPECT_EQ((1 * v3), v3);
    EXPECT_EQ((2 * v3), Vec3f({2, 4, 6}));
    EXPECT_EQ((2 * v3), (v3 * 2));
    EXPECT_EQ((-2 * v3), Vec3f({-2, -4, -6}));
    EXPECT_EQ((-2 * v3), (v3 * -2));

    EXPECT_EQ((-1 * v3), -v3);

    EXPECT_EQ((v4 / 1), v4);
    EXPECT_EQ((v4 / 2), Vec3f({2.f, 2.5f, 3.0f}));
}

using mathengine::Matrix22f;
using mathengine::Matrix33f;
using mathengine::Matrix44f;

class MatrixTest : public testing::Test {
protected:
    void SetUp() override
    {
        mat1 = Matrix44f{1, 0, 0, 0,
                         0, 1, 0, 0,
                         0, 0, 1, 0,
                         0, 0, 0, 1};
        mat2 = Matrix44f{1,   2,  3,  4,
                         5,   6,  7,  8,
                         9,  10, 11, 12,
                         13, 14, 15, 16};
        mat3 = Matrix44f{-1,  -2,  -3,  -4,
                         -5,  -6,  -7,  -8,
                         -9, -10, -11, -12,
                        -13, -14, -15, -16};
        mat4 = Matrix44f{2,  4,  6,  8,
                        10, 12, 14, 16,
                        18, 20, 22, 24,
                        26, 28, 30, 32};
    }

    void TearDown() override {
    }

    Matrix44f mat1;
    Matrix44f mat2;
    Matrix44f mat3;
    Matrix44f mat4;
};

TEST_F(MatrixTest, DefaultConstructor) {
    EXPECT_EQ(mat1, Matrix44f::identity());
    EXPECT_EQ(0, -0);
    EXPECT_EQ(0, -1 * 0);
}

TEST_F(MatrixTest, ValueAccessor) {
    auto tmpmat = mat2;
    for (int i = 0, t = 1; i < tmpmat.getRowDim(); i++) {
        for (int j = 0; j < tmpmat.getColDim(); j++) {
            EXPECT_EQ(tmpmat.getValue(i, j), t);
            t++;
        }
    }

    tmpmat.setValue(1, 1, 100);
    EXPECT_EQ(tmpmat.getValue(1, 1), 100);

    tmpmat.setValue(2, 3, -200);
    EXPECT_EQ(tmpmat.getValue(2, 3), -200);
}

TEST_F(MatrixTest, RowOrColAccessor) {
    // col
    auto tmpmat = mat2;
    EXPECT_EQ(tmpmat.getCol(0), (Vec4f{1, 5,  9, 13}));
    EXPECT_EQ(tmpmat.getCol(1), (Vec4f{2, 6, 10, 14}));
    EXPECT_EQ(tmpmat.getCol(2), (Vec4f{3, 7, 11, 15}));
    EXPECT_EQ(tmpmat.getCol(3), (Vec4f{4, 8, 12, 16}));

    tmpmat.setCol(0, Vec4f{-1, -5,  -9, -13});
    tmpmat.setCol(1, Vec4f{-2, -6, -10, -14});
    tmpmat.setCol(2, Vec4f{-3, -7, -11, -15});
    tmpmat.setCol(3, Vec4f{-4, -8, -12, -16});

    EXPECT_EQ(tmpmat.getCol(0), (Vec4f{-1, -5,  -9, -13}));
    EXPECT_EQ(tmpmat.getCol(1), (Vec4f{-2, -6, -10, -14}));
    EXPECT_EQ(tmpmat.getCol(2), (Vec4f{-3, -7, -11, -15}));
    EXPECT_EQ(tmpmat.getCol(3), (Vec4f{-4, -8, -12, -16}));

    // row
    tmpmat = mat2;
    EXPECT_EQ(tmpmat.getRow(0), (Vec4f{1,   2,  3,  4}));
    EXPECT_EQ(tmpmat.getRow(1), (Vec4f{5,   6,  7,  8}));
    EXPECT_EQ(tmpmat.getRow(2), (Vec4f{9,  10, 11, 12}));
    EXPECT_EQ(tmpmat.getRow(3), (Vec4f{13, 14, 15, 16}));

    tmpmat.setRow(0, Vec4f{-1, -5,  -9, -13});
    tmpmat.setRow(1, Vec4f{-2, -6, -10, -14});
    tmpmat.setRow(2, Vec4f{-3, -7, -11, -15});
    tmpmat.setRow(3, Vec4f{-4, -8, -12, -16});

    EXPECT_EQ(tmpmat.getRow(0), (Vec4f{-1, -5,  -9, -13}));
    EXPECT_EQ(tmpmat.getRow(1), (Vec4f{-2, -6, -10, -14}));
    EXPECT_EQ(tmpmat.getRow(2), (Vec4f{-3, -7, -11, -15}));
    EXPECT_EQ(tmpmat.getRow(3), (Vec4f{-4, -8, -12, -16}));
}

TEST_F(MatrixTest, AddAndSub) {
    EXPECT_EQ(mat1, mat1);
    EXPECT_EQ(mat1 + Matrix44f::zero(), mat1);
    EXPECT_EQ(mat2 + mat3, Matrix44f::zero());
    EXPECT_EQ(mat1 - mat1, Matrix44f::zero());
    EXPECT_EQ(mat2 - mat2, Matrix44f::zero());

    auto tmpmat1 = mat1;
    auto tmpmat2 = mat2;
    auto tmpmat3 = mat3;

    EXPECT_EQ(tmpmat1, mat1);
    EXPECT_EQ(tmpmat2, mat2);
    EXPECT_EQ(tmpmat3, mat3);

    tmpmat1 += mat1;
    tmpmat2 += mat2;
    tmpmat3 += mat3;

    EXPECT_EQ(tmpmat1, mat1 + mat1);
    EXPECT_EQ(tmpmat2, mat2 + mat2);
    EXPECT_EQ(tmpmat3, mat3 + mat3);

    tmpmat1 -= mat1;
    tmpmat2 -= mat2;
    tmpmat3 -= mat3;

    EXPECT_EQ(tmpmat1, mat1);
    EXPECT_EQ(tmpmat2, mat2);
    EXPECT_EQ(tmpmat3, mat3);
}

TEST_F(MatrixTest, NumMultiplyAndNumDiv) {
    EXPECT_EQ(0.0f * mat2, Matrix44f::zero());

    EXPECT_EQ(1.0f * mat2, mat2);
    EXPECT_EQ(-1.0f * mat2, mat3);
    EXPECT_EQ(2.0f * mat2, mat4);
    EXPECT_EQ(-2.0f * mat3, mat4);

    EXPECT_EQ(mat2 / 1, mat2);
    EXPECT_EQ(mat2 / -1, mat3);
    EXPECT_EQ(mat4 / 2, mat2);
}

TEST_F(MatrixTest, VectorMultiply) {
    Vec4f a, b;
    a = mat1 * Vec4f::zero();
    b = Vec4f::zero();
    EXPECT_EQ(a, b);

    a = mat1 * Vec4f{1, 1, 1, 1};
    b = Vec4f{ 1, 1, 1, 1};
    EXPECT_EQ(a, b);

    a = mat2 * Vec4f{-2, 4, 11, 5};
    b = Vec4f{59, 131, 203, 275};
    EXPECT_EQ(a, b);
}

TEST_F(MatrixTest, SquareMatrixMultiply) {
    EXPECT_EQ(dotProduct(Matrix44f::zero(), mat2), Matrix44f::zero());
    EXPECT_EQ(dotProduct(mat2, Matrix44f::zero()), Matrix44f::zero());

    EXPECT_EQ(dotProduct(mat1, mat2), mat2);
    EXPECT_EQ(dotProduct(mat2, mat1), mat2);

    Matrix44f tmpmat1({  11,  13,  15,  17,
                         19,  21,  23,  25,
                        -27, -29, -31, -33,
                        -35, -37, -39, -41 });
    Matrix44f tmpmat2{ -172, -180, -188, -196,
                       -300, -308, -316, -324,
                       -428, -436, -444, -452,
                       -556, -564, -572, -580 };
    Matrix44f tmpmat3{ 432,    488,   544,   600,
                       656,    744,   832,   920,
                      -880,  -1000, -1120, -1240,
                      -1104, -1256, -1408, -1560 };

    EXPECT_EQ(dotProduct(mat2, tmpmat1), tmpmat2);
    EXPECT_EQ(dotProduct(tmpmat1, mat2), tmpmat3);
}

TEST_F(MatrixTest, GeneralMatrixMultiply) {
    Mat<float, 3, 4> mat1{ 1,  2,  3,  4,
                           5,  6,  7,  8,
                           9, 10, 11, 12 };
    Mat<float, 4, 3> mat2{  3,   5,   7,
                            9,  11,  13,
                          -15, -17, -19,
                          -21, -23, -25 };
    Mat<float, 3, 3> mat3{ -108, -116, -124,
                           -204, -212, -220,
                           -300, -308, -316 };
    EXPECT_EQ(mat1 * mat2, mat3);
}

TEST_F(MatrixTest, Transpose) {
    EXPECT_EQ(mat1.transposed(), Matrix44f::identity());
    EXPECT_EQ(mat2.transposed(), Matrix44f({ 1, 5,  9, 13,
                                             2, 6, 10, 14,
                                             3, 7, 11, 15,
                                             4, 8, 12, 16 }));
    EXPECT_EQ(mat2.transposed().transposed(), mat2);

    EXPECT_EQ(mat3.transposed(), Matrix44f({ -1, -5,  -9, -13,
                                             -2, -6, -10, -14,
                                             -3, -7, -11, -15,
                                             -4, -8, -12, -16 }));
    EXPECT_EQ(mat3.transposed().transposed(), mat3);

    auto t1 = mat2;
    EXPECT_TRUE(t1 == mat2);

    t1.transpose();
    EXPECT_FALSE(t1 == mat2);

    t1.transpose();
    EXPECT_TRUE(t1 == mat2);

    auto t2 = Mat<float, 2, 4>{ 1, 2, 3, 4,
                                5, 6, 7, 8 };
    auto t3 = Mat<float, 4, 2>{ 1, 5,
                                2, 6,
                                3, 7,
                                4, 8 };
    EXPECT_EQ(t2.transposed(), t3);
    EXPECT_EQ(t2, t3.transposed());
}

TEST_F(MatrixTest, Det) {
    EXPECT_EQ(Matrix44f::zero().det(), 0);
    EXPECT_EQ(mat1.det(), 1);
    EXPECT_EQ(mat2.det(), 0);
    EXPECT_EQ(mat3.det(), 0);
    EXPECT_EQ(Matrix44f({ 1,   2,   5,  0,
                          0, 100,  13, 17,
                          0,  11,   1, 19,
                          0, -20, -15, 1 })
                  .det(),
              21052);
}

// type float may cause the accuracy problem
TEST_F(MatrixTest, Inverse)
{
    EXPECT_EQ(mat1.inverse(), mat1);

    Matrix44f t1 = Matrix44f({ 1,    2,   5,  0,
                               0,  100,  13, 17,
                               0,   11,   1, 19,
                               0,  -20, -15,  1 });
    Matrix44f t2({1, 0.06569447083412502, -0.07904237127113814, 0.38499904997149914,
                  0, 0.013585407562226866, -0.012730381911457346, 0.010925327759832796,
                  0, -0.01857305719171575, 0.020900627018810566, -0.08136994109823295,
                  0, -0.006887706631198936, 0.058901767053011596, -0.0020425612768383058});

    EXPECT_EQ(t1.inverse(), t2);
    EXPECT_EQ(t1, t2.inverse());
    EXPECT_EQ(dotProduct(t1, t2), Matrix44f::identity());

    t1 = Matrix44f({  2,  4,   1, -100,
                    -11,  5,   7,    0,
                      0, 87, -99,    0,
                    -40, 45, -20,    9 });
    t2 = Matrix44f({-0.00729616664867593, 0.202558945404319, 0.0306261294702422, -0.0810685183186214,
                    -0.00719703394964501, 0.289480698863499, 0.0365505932908458, -0.0799670438849445,
                    -0.00632466619817288, 0.254392129304287, 0.0220192082454908, -0.0702740688685876,
                    -0.0104970513529411, 0.0181743281556692, 0.00229473840349359, -0.00552279281045608});

    EXPECT_EQ(t1.inverse(), t2);
    //    EXPECT_EQ(m1, m2.inverse()); // maybe exist accuracy problem
    EXPECT_EQ(dotProduct(t1, t2), Matrix44f::identity());

    t1 = Matrix44f({ 1, 2, 0, 1,
                     0, 1, 1, 0,
                     2, 0, 1, 1,
                     1, 1, 0, 1 });
    t2 = Matrix44f({ 2, -1,  1, -3,
                     1,  0,  0, -1,
                    -1,  1,  0,  1,
                    -3,  1, -1,  5});
    EXPECT_EQ(t1.inverse(), t2);
    EXPECT_EQ(t1, t2.inverse());
    EXPECT_TRUE(dotProduct(t1, t2) == Matrix44f::identity());

    auto t3 = t1;
    auto t4 = t2;

    EXPECT_TRUE(dotProduct(t3, t4) == Matrix44f::identity());
    t3.invert();
    EXPECT_FALSE(dotProduct(t3, t4) == Matrix44f::identity());

    t3.invert();
    EXPECT_TRUE(dotProduct(t3, t4) == Matrix44f::identity());
}

} // namespace
